﻿#define DEBUG_DRAW

using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using PriorityQueue;
using System;
using System.Runtime.InteropServices;
using System.Linq;


namespace PathFinding
{
    public class JPSPathFinder : IPathFInder
    {

        //Debug Draw
        public List<Vector2Int> OpenedPoints { get; private set; }
        public List<Vector2Int> OtherClosedPoints { get; private set; }

        // end debug draw

        public List<Vector2Int> CurrentPath { get; private set; }

        /// <summary>
        /// 可以在单位行动时逐步处理
        /// </summary>
        public List<Vector2Int> SmoothedPath { get; private set; }

        PFGrid grid;

        struct JPSNode
        {

            public bool IsBlocked;
            public JPSNode(bool blocked)
            {
                this.IsBlocked = blocked;
            }
        }

        bool[,] closed;
        JPSNode[,] jpsNodes;
        PFNode[,] nodes;
        PFNodeHeap openHeap;
        int mapXSize, mapYSize;
        int goal_index;


        public bool IsWorking { get; private set; }

        public JPSPathFinder(PFGrid pFGrid)
        {
            grid = pFGrid;
            mapXSize = grid.XSize;
            mapYSize = grid.YSize;
            goal_index = -1;
            closed = new bool[mapXSize, mapYSize];

            nodes = new PFNode[mapXSize, mapYSize];
            jpsNodes = new JPSNode[mapXSize, mapYSize];
            for (int x = 0; x < mapXSize; ++x)
            {
                for (int y = 0; y < mapYSize; ++y)
                {
                    nodes[x, y] = new PFNode(new Vector2Int(x, y), grid[x, y]);
                }
            }

            IsWorking = false;

            // debug draw
            OpenedPoints = new List<Vector2Int>(1024);
            OtherClosedPoints = new List<Vector2Int>(1024);

        }

        private void CleanLocalList()
        {
            Array.Clear(closed, 0, closed.Length);
            CurrentPath = null;
            openHeap = null;
        }

        [System.Runtime.CompilerServices.MethodImpl(System.Runtime.CompilerServices.MethodImplOptions.AggressiveInlining)]
        private void SetClosed(in Vector2Int pos)
        {
            closed[pos.x, pos.y] = true;
        }

        [System.Runtime.CompilerServices.MethodImpl(System.Runtime.CompilerServices.MethodImplOptions.AggressiveInlining)]
        private bool IsClosed(in Vector2Int pos)
        {
            return closed[pos.x, pos.y];
        }

        public bool FindPath(Vector2Int start, Vector2Int goal, bool postSmooth = true)
        {
            if (IsWorking)
                throw new Exception("Can't init a new path finding now");
#if DEBUG_DRAW
            OpenedPoints.Clear();
            OtherClosedPoints.Clear();
#endif

            System.Diagnostics.Stopwatch sw = new System.Diagnostics.Stopwatch();
            sw.Start();

            IsWorking = true;
            CleanLocalList();

            openHeap = new PFNodeHeap();
            PFNode startNode = nodes[start.x, start.y];
            startNode.gCost = 0;
            startNode.hCost = 0;
            startNode.parent = null;
            openHeap.Add(startNode);

            goal_index = goal.y * mapXSize + goal.x;

#if DEBUG_DRAW
            OpenedPoints.Add(startNode.pos);
#endif

            while (openHeap.Count != 0)
            {
                PFNode nodeToVisit = openHeap.Pop();

                /// we found path
                if (nodeToVisit.pos == goal)
                {
                    sw.Stop();
                    Debug.Log("PathFind总共花费: " + sw.Elapsed.TotalMilliseconds + " ms");
#if DEBUG_DRAW
                    Debug.Log("Open: " + OpenedPoints.Count + " Closed: " + OtherClosedPoints.Count);
#endif

                    CurrentPath = new List<Vector2Int>(2);
                    while (nodeToVisit.pos != start && nodeToVisit.parent != null)
                    {
                        CurrentPath.Add(nodeToVisit.pos);
                        nodeToVisit = nodeToVisit.parent;
                    }
                    CurrentPath.Add(start);

                    CurrentPath.Reverse();

                    // post smooth
                    if (postSmooth)
                    {
                        SmoothedPath = new List<Vector2Int>(2);

                        SmoothedPath.Add(CurrentPath.First());
                        var cs = CurrentPath[0];
                        for (var i = 1; i < CurrentPath.Count - 1; ++i)
                        {
                            if (!grid.IsLineOfSight(cs, CurrentPath[i + 1]))
                            {
                                SmoothedPath.Add(CurrentPath[i]);
                                cs = CurrentPath[i];
                            }
                        }

                        SmoothedPath.Add(CurrentPath.Last());
                    }

                    IsWorking = false;
                    return true;
                }

                if (goal.x > nodeToVisit.pos.x)
                {
                    JumpE(nodeToVisit.pos, goal, nodeToVisit.pos, false); // 向右
                    if (goal.y > nodeToVisit.pos.y)
                    {
                        JumpN(nodeToVisit.pos, goal, nodeToVisit.pos, false); // 向上
                        JumpNE(nodeToVisit.pos, goal); // 右上
                        JumpW(nodeToVisit.pos, goal, nodeToVisit.pos, false); // 向左
                        JumpNW(nodeToVisit.pos, goal); // 左上

                        JumpS(nodeToVisit.pos, goal, nodeToVisit.pos, false); // 向下
                        JumpSE(nodeToVisit.pos, goal); // 右下
                        JumpSW(nodeToVisit.pos, goal); // 左下

                    }
                    else
                    {
                        JumpS(nodeToVisit.pos, goal, nodeToVisit.pos, false); // 向下
                        JumpSE(nodeToVisit.pos, goal); // 右下
                        JumpW(nodeToVisit.pos, goal, nodeToVisit.pos, false); // 向左
                        JumpSW(nodeToVisit.pos, goal); // 左下

                        JumpN(nodeToVisit.pos, goal, nodeToVisit.pos, false); // 向上
                        JumpNE(nodeToVisit.pos, goal); // 右上
                        JumpNW(nodeToVisit.pos, goal); // 左上
                    }
                }
                else
                {
                    JumpW(nodeToVisit.pos, goal, nodeToVisit.pos, false); // 向左
                    if (goal.y > nodeToVisit.pos.y)
                    {
                        JumpN(nodeToVisit.pos, goal, nodeToVisit.pos, false); // 向上
                        JumpNW(nodeToVisit.pos, goal); // 左上
                        JumpE(nodeToVisit.pos, goal, nodeToVisit.pos, false); //向右
                        JumpNE(nodeToVisit.pos, goal); //右上

                        JumpS(nodeToVisit.pos, goal, nodeToVisit.pos, false); // 向下
                        JumpSW(nodeToVisit.pos, goal); // 左下
                        JumpSE(nodeToVisit.pos, goal); // 右下
                    }
                    else
                    {
                        JumpS(nodeToVisit.pos, goal, nodeToVisit.pos, false); // 向下
                        JumpSW(nodeToVisit.pos, goal); // 左下
                        JumpE(nodeToVisit.pos, goal, nodeToVisit.pos, false); //向右
                        JumpSE(nodeToVisit.pos, goal); // 右下

                        JumpN(nodeToVisit.pos, goal, nodeToVisit.pos, false); // 向上
                        JumpNW(nodeToVisit.pos, goal); // 左上
                        JumpNE(nodeToVisit.pos, goal); // 右上
                    }
                }

                SetClosed(nodeToVisit.pos);

            }

            CurrentPath = null;
            SmoothedPath = null;
#if DEBUG_DRAW
            OpenedPoints.Clear();
            OtherClosedPoints.Clear();
            Debug.Log("Open: " + OpenedPoints.Count + " Closed: " + OtherClosedPoints.Count);
#endif
            IsWorking = false;

            sw.Stop();
            Debug.Log("PathFind总共花费: " + sw.Elapsed.TotalMilliseconds + " ms");

            return false;
        }

        [System.Runtime.CompilerServices.MethodImpl(System.Runtime.CompilerServices.MethodImplOptions.AggressiveInlining)]
        private int DistCost(Vector2Int start, Vector2Int end)
        {
            int dx = Mathf.Abs(start.x - end.x);
            int dy = Mathf.Abs(start.y - end.y);

            return Mathf.Abs(dx - dy) * 10 + (dx > dy ? dy : dx) * 14;
        }

        /// <summary>
        /// 比g值计算方式略大，用以加速
        /// </summary>
        const int HValueMul = 13;
        /// <summary>
        /// 比g值计算方式略大，用以加速
        /// </summary>
        const int HValueMulDiagonal = 18;

        /// <summary>
        /// H 用于计算启发Cost 
        /// 即h值
        /// </summary>
        static int PredictDistanceCost(Vector2Int start, Vector2Int end)
        {
            //return Math.Abs(start.x - end.x) + Math.Abs(start.y - end.y); // 曼哈顿

            //return (int)((start - end).magnitude * 10); // 直线



            // 八向距离
            int dx = Mathf.Abs(start.x - end.x);
            int dy = Mathf.Abs(start.y - end.y);

            return Mathf.Abs(dx - dy) * HValueMul + (dx > dy ? dy : dx) * HValueMulDiagonal;

        }

        void TryAddOpenNode(in Vector2Int npos, in Vector2Int from, in Vector2Int goal)
        {
            var adjacent = nodes[npos.x, npos.y];

            PFNode fromNode = nodes[from.x, from.y];

            int toAdjacentCost = fromNode.gCost + DistCost(fromNode.pos, npos);
            if (!openHeap.Contains(adjacent) || toAdjacentCost < adjacent.gCost)
            {
                adjacent.gCost = toAdjacentCost;
                adjacent.hCost = PredictDistanceCost(npos, goal);
                adjacent.parent = fromNode;

                if (openHeap.Contains(adjacent))
                    openHeap.UpdateItem(adjacent);
                else
                {
#if DEBUG_DRAW
                    OpenedPoints.Add(adjacent.pos);
#endif
                    openHeap.Add(adjacent);
                }
            }
        }

        [System.Runtime.CompilerServices.MethodImpl(System.Runtime.CompilerServices.MethodImplOptions.AggressiveInlining)]
        public bool HasPointAt(in Vector2Int pos)
        {
            if (pos.x >= 0 && pos.x < mapXSize && pos.y >= 0 && pos.y < mapYSize)
                return true;
            return false;
        }

        [System.Runtime.CompilerServices.MethodImpl(System.Runtime.CompilerServices.MethodImplOptions.AggressiveInlining)]
        public bool HasPointAt(int x, int y)
        {
            if (x >= 0 && x < mapXSize && y >= 0 && y < mapYSize)
                return true;
            return false;
        }


        /// <summary>
        /// 向北跳跃
        /// </summary>
        bool JumpN(in Vector2Int from, in Vector2Int goal, in Vector2Int fromParent, bool byDiagonal)
        {
            Vector2Int curr = from;
            if (curr.x != 0) // 不在左侧边界，左侧拓展一格检测碰撞是没问题的
            {
                if (curr.x != mapXSize - 1) //左侧和右侧拓展一格检测碰撞是没问题的
                {
                    while (true)
                    {
                        curr.y -= 1;
                        if (curr.y < 0 || jpsNodes[curr.x, curr.y].IsBlocked || closed[curr.x, curr.y])
                        {
                            // 到了地图边缘或者前方被阻挡或者已经探查过
                            // 跳点结束
                            return false;
                        }

                        if (curr == goal)
                        {
                            if (byDiagonal)
                                TryAddOpenNode(from, fromParent, goal);

                            TryAddOpenNode(curr, from, goal);
                            return true;
                        }

                        if ((jpsNodes[curr.x + 1, curr.y + 1].IsBlocked && !jpsNodes[curr.x + 1, curr.y].IsBlocked) ||
                            (jpsNodes[curr.x - 1, curr.y + 1].IsBlocked && !jpsNodes[curr.x - 1, curr.y].IsBlocked))
                        {
                            // 这个点侧边有障碍或者是目标点，则认为这个点是关键点。
                            // 如果是被对角线跳点调用的，需要先保证对角线的当前点，也就是直线跳点的起点加入OpenList
                            if (byDiagonal)
                                TryAddOpenNode(from, fromParent, goal);

                            TryAddOpenNode(curr, from, goal);
                            return true;
                        }

                        closed[curr.x, curr.y] = true;
                    }
                }
                else // 在右侧边缘，不能检测右侧碰撞, x不可再增加
                {
                    while (true)
                    {
                        curr.y -= 1;
                        if (curr.y < 0 || jpsNodes[curr.x, curr.y].IsBlocked || closed[curr.x, curr.y])
                        {
                            return false;
                        }

                        if (curr == goal)
                        {
                            if (byDiagonal)
                                TryAddOpenNode(from, fromParent, goal);

                            TryAddOpenNode(curr, from, goal);
                            return true;
                        }

                        if (jpsNodes[curr.x - 1, curr.y + 1].IsBlocked && !jpsNodes[curr.x - 1, curr.y].IsBlocked)
                        {
                            if (byDiagonal)
                                TryAddOpenNode(from, fromParent, goal);

                            TryAddOpenNode(curr, from, goal);
                            return true;
                        }

                        closed[curr.x, curr.y] = true;
                    }
                }
            }
            else // 在地图左侧边缘，不能检测左侧碰撞, x == 0, x不能载减少
            {
                while (true)
                {
                    curr.y -= 1;
                    if (curr.y < 0 || jpsNodes[curr.x, curr.y].IsBlocked || closed[curr.x, curr.y])
                    {
                        return false;
                    }

                    if (curr == goal)
                    {
                        if (byDiagonal)
                            TryAddOpenNode(from, fromParent, goal);

                        TryAddOpenNode(curr, from, goal);
                        return true;
                    }

                    if (jpsNodes[curr.x + 1, curr.y + 1].IsBlocked && !jpsNodes[curr.x + 1, curr.y].IsBlocked)
                    {
                        if (byDiagonal)
                            TryAddOpenNode(from, fromParent, goal);

                        TryAddOpenNode(curr, from, goal);
                        return true;
                    }

                    closed[curr.x, curr.y] = true;
                }
            }
        }

        /// <summary>
        /// 向东跳跃
        /// </summary>
        bool JumpE(in Vector2Int from, in Vector2Int goal, in Vector2Int fromParent, bool byDiagonal)
        {
            Vector2Int curr = from;
            if (curr.y != 0) // 不在上边缘
            {
                if (curr.y != mapYSize - 1) // 不在下边缘, 两侧都能检索
                {
                    while (true)
                    {
                        curr.x += 1;
                        if (curr.x >= mapXSize || jpsNodes[curr.x, curr.y].IsBlocked || closed[curr.x, curr.y])
                        {
                            return false;
                        }

                        if (curr == goal)
                        {
                            if (byDiagonal)
                                TryAddOpenNode(from, fromParent, goal);

                            TryAddOpenNode(curr, from, goal);
                            return true;
                        }

                        if ((jpsNodes[curr.x - 1, curr.y + 1].IsBlocked && !jpsNodes[curr.x, curr.y + 1].IsBlocked) ||
                            (jpsNodes[curr.x - 1, curr.y - 1].IsBlocked && !jpsNodes[curr.x, curr.y - 1].IsBlocked))
                        {
                            if (byDiagonal)
                                TryAddOpenNode(from, fromParent, goal);

                            TryAddOpenNode(curr, from, goal);
                            return true;
                        }

                        closed[curr.x, curr.y] = true;
                    }
                }
                else // 在下边缘，检测上边 y -
                {
                    while (true)
                    {
                        curr.x += 1;
                        if (curr.x >= mapXSize || jpsNodes[curr.x, curr.y].IsBlocked || closed[curr.x, curr.y])
                        {
                            return false;
                        }

                        if (curr == goal)
                        {
                            if (byDiagonal)
                                TryAddOpenNode(from, fromParent, goal);

                            TryAddOpenNode(curr, from, goal);
                            return true;
                        }

                        if (jpsNodes[curr.x - 1, curr.y - 1].IsBlocked && !jpsNodes[curr.x, curr.y - 1].IsBlocked)
                        {
                            if (byDiagonal)
                                TryAddOpenNode(from, fromParent, goal);

                            TryAddOpenNode(curr, from, goal);
                            return true;
                        }

                        closed[curr.x, curr.y] = true;
                    }
                }
            }
            else // 在上边缘y==0，检测下边 y +
            {
                while (true)
                {
                    curr.x += 1;
                    if (curr.x >= mapXSize || jpsNodes[curr.x, curr.y].IsBlocked || closed[curr.x, curr.y])
                    {
                        return false;
                    }

                    if (curr == goal)
                    {
                        if (byDiagonal)
                            TryAddOpenNode(from, fromParent, goal);

                        TryAddOpenNode(curr, from, goal);
                        return true;
                    }

                    if (jpsNodes[curr.x - 1, curr.y + 1].IsBlocked && !jpsNodes[curr.x, curr.y + 1].IsBlocked)
                    {
                        if (byDiagonal)
                            TryAddOpenNode(from, fromParent, goal);

                        TryAddOpenNode(curr, from, goal);
                        return true;
                    }

                    closed[curr.x, curr.y] = true;
                }
            }
        }

        /// <summary>
        /// 向南跳跃
        /// </summary>
        bool JumpS(in Vector2Int from, in Vector2Int goal, in Vector2Int fromParent, bool byDiagonal)
        {
            Vector2Int curr = from;
            if (curr.x != 0) // 不在左边缘
            {
                if (curr.x != mapXSize - 1) // 不在右边缘
                {
                    while (true)
                    {
                        curr.y += 1;
                        if (curr.y >= mapYSize || jpsNodes[curr.x, curr.y].IsBlocked || closed[curr.x, curr.y])
                        {
                            return false;
                        }

                        if (curr == goal)
                        {
                            if (byDiagonal)
                                TryAddOpenNode(from, fromParent, goal);

                            TryAddOpenNode(curr, from, goal);
                            return true;
                        }

                        if ((jpsNodes[curr.x - 1, curr.y - 1].IsBlocked && !jpsNodes[curr.x - 1, curr.y].IsBlocked) ||
                            (jpsNodes[curr.x + 1, curr.y - 1].IsBlocked && !jpsNodes[curr.x + 1, curr.y].IsBlocked))
                        {
                            if (byDiagonal)
                                TryAddOpenNode(from, fromParent, goal);

                            TryAddOpenNode(curr, from, goal);
                            return true;
                        }

                        closed[curr.x, curr.y] = true;
                    }
                }
                else // x == mapXSize - 1 在右边缘, x只能-1
                {
                    while (true)
                    {
                        curr.y += 1;
                        if (curr.y >= mapYSize || jpsNodes[curr.x, curr.y].IsBlocked || closed[curr.x, curr.y])
                        {
                            return false;
                        }

                        if (curr == goal)
                        {
                            if (byDiagonal)
                                TryAddOpenNode(from, fromParent, goal);

                            TryAddOpenNode(curr, from, goal);
                            return true;
                        }

                        if (jpsNodes[curr.x - 1, curr.y - 1].IsBlocked && !jpsNodes[curr.x - 1, curr.y].IsBlocked)
                        {
                            if (byDiagonal)
                                TryAddOpenNode(from, fromParent, goal);

                            TryAddOpenNode(curr, from, goal);
                            return true;
                        }

                        closed[curr.x, curr.y] = true;
                    }
                }
            }
            else // x==0在左边缘，x只能+1
            {
                while (true)
                {
                    curr.y += 1;
                    if (curr.y >= mapYSize || jpsNodes[curr.x, curr.y].IsBlocked || closed[curr.x, curr.y])
                    {
                        return false;
                    }

                    if (curr == goal)
                    {
                        if (byDiagonal)
                            TryAddOpenNode(from, fromParent, goal);

                        TryAddOpenNode(curr, from, goal);
                        return true;
                    }

                    if (jpsNodes[curr.x + 1, curr.y - 1].IsBlocked && !jpsNodes[curr.x + 1, curr.y].IsBlocked)
                    {
                        if (byDiagonal)
                            TryAddOpenNode(from, fromParent, goal);

                        TryAddOpenNode(curr, from, goal);
                        return true;
                    }

                    closed[curr.x, curr.y] = true;
                }
            }
        }

        /// <summary>
        /// 向西跳跃
        /// </summary>
        bool JumpW(in Vector2Int from, in Vector2Int goal, in Vector2Int fromParent, bool byDiagonal)
        {
            Vector2Int curr = from;
            if (curr.y != 0)
            {
                if (curr.y != mapYSize - 1)
                {
                    while (true)
                    {
                        curr.x -= 1;
                        if (curr.x < 0 || jpsNodes[curr.x, curr.y].IsBlocked || closed[curr.x, curr.y])
                        {
                            return false;
                        }

                        if (curr == goal)
                        {
                            if (byDiagonal)
                                TryAddOpenNode(from, fromParent, goal);

                            TryAddOpenNode(curr, from, goal);
                            return true;
                        }

                        if ((jpsNodes[curr.x + 1, curr.y - 1].IsBlocked && !jpsNodes[curr.x, curr.y - 1].IsBlocked) ||
                            (jpsNodes[curr.x + 1, curr.y + 1].IsBlocked && !jpsNodes[curr.x, curr.y + 1].IsBlocked))
                        {
                            if (byDiagonal)
                                TryAddOpenNode(from, fromParent, goal);

                            TryAddOpenNode(curr, from, goal);
                            return true;
                        }

                        closed[curr.x, curr.y] = true;
                    }
                }
                else // y = mapYSize , y-1
                {
                    while (true)
                    {
                        curr.x -= 1;
                        if (curr.x < 0 || jpsNodes[curr.x, curr.y].IsBlocked || closed[curr.x, curr.y])
                        {
                            return false;
                        }

                        if (curr == goal)
                        {
                            if (byDiagonal)
                                TryAddOpenNode(from, fromParent, goal);

                            TryAddOpenNode(curr, from, goal);
                            return true;
                        }

                        if (jpsNodes[curr.x + 1, curr.y - 1].IsBlocked && !jpsNodes[curr.x, curr.y - 1].IsBlocked)
                        {
                            if (byDiagonal)
                                TryAddOpenNode(from, fromParent, goal);

                            TryAddOpenNode(curr, from, goal);
                            return true;
                        }

                        closed[curr.x, curr.y] = true;
                    }
                }
            }
            else // y == 0, y+1
            {
                while (true)
                {
                    curr.x -= 1;
                    if (curr.x < 0 || jpsNodes[curr.x, curr.y].IsBlocked || closed[curr.x, curr.y])
                    {
                        return false;
                    }

                    if (curr == goal)
                    {
                        if (byDiagonal)
                            TryAddOpenNode(from, fromParent, goal);

                        TryAddOpenNode(curr, from, goal);
                        return true;
                    }

                    if (jpsNodes[curr.x + 1, curr.y + 1].IsBlocked && !jpsNodes[curr.x, curr.y + 1].IsBlocked)
                    {
                        if (byDiagonal)
                            TryAddOpenNode(from, fromParent, goal);

                        TryAddOpenNode(curr, from, goal);
                        return true;
                    }

                    closed[curr.x, curr.y] = true;
                }
            }
        }


        /// <summary>
        /// 向东北跳跃
        /// </summary>
        void JumpNE(in Vector2Int from, in Vector2Int goal)
        {
            Vector2Int curr = from;
            while (true)
            {
                curr.y -= 1;
                curr.x += 1;
                if (!HasPointAt(curr) || jpsNodes[curr.x, curr.y].IsBlocked || closed[curr.x, curr.y])
                {
                    // 到了地图边缘或者前方被阻挡或者已经探查过
                    // 跳点结束
                    return;
                }

                if (curr == goal)
                {
                    TryAddOpenNode(curr, from, goal);
                    return;
                }

                bool HasOpenNode = false;
                if (JumpN(curr, goal, from, true))
                    HasOpenNode = true;
                if (JumpE(curr, goal, from, true))
                    HasOpenNode = true;

                if (!HasOpenNode)
                    closed[curr.x, curr.y] = true;
                else
                    return;
            }
        }

        /// <summary>
        /// 向东南跳跃
        /// </summary>
        void JumpSE(in Vector2Int from, in Vector2Int goal)
        {
            Vector2Int curr = from;
            while (true)
            {
                curr.y += 1;
                curr.x += 1;
                if (!HasPointAt(curr) || jpsNodes[curr.x, curr.y].IsBlocked || closed[curr.x, curr.y])
                {
                    // 到了地图边缘或者前方被阻挡或者已经探查过
                    // 跳点结束
                    return;
                }

                if (curr == goal)
                {
                    TryAddOpenNode(curr, from, goal);
                    return;
                }

                bool HasOpenNode = false;
                if (JumpE(curr, goal, from, true))
                    HasOpenNode = true;
                if (JumpS(curr, goal, from, true))
                    HasOpenNode = true;

                if (!HasOpenNode)
                    closed[curr.x, curr.y] = true;
                else
                    return;
            }
        }

        /// <summary>
        /// 向西南跳跃
        /// </summary>
        void JumpSW(in Vector2Int from, in Vector2Int goal)
        {
            Vector2Int curr = from;
            while (true)
            {
                curr.y += 1;
                curr.x -= 1;
                if (!HasPointAt(curr) || jpsNodes[curr.x, curr.y].IsBlocked || closed[curr.x, curr.y])
                {
                    // 到了地图边缘或者前方被阻挡或者已经探查过
                    // 跳点结束
                    return;
                }

                if (curr == goal)
                {
                    TryAddOpenNode(curr, from, goal);
                    return;
                }

                bool HasOpenNode = false;
                if (JumpS(curr, goal, from, true))
                    HasOpenNode = true;
                if (JumpW(curr, goal, from, true))
                    HasOpenNode = true;

                if (!HasOpenNode)
                    closed[curr.x, curr.y] = true;
                else
                    return;
            }
        }

        /// <summary>
        /// 向西北跳跃
        /// </summary>
        void JumpNW(in Vector2Int from, in Vector2Int goal)
        {
            Vector2Int curr = from;
            while (true)
            {
                curr.y -= 1;
                curr.x -= 1;
                if (!HasPointAt(curr) || jpsNodes[curr.x, curr.y].IsBlocked || closed[curr.x, curr.y])
                {
                    // 到了地图边缘或者前方被阻挡或者已经探查过
                    // 跳点结束
                    return;
                }

                if (curr == goal)
                {
                    TryAddOpenNode(curr, from, goal);
                    return;
                }

                bool HasOpenNode = false;
                if (JumpW(curr, goal, from, true))
                    HasOpenNode = true;
                if (JumpN(curr, goal, from, true))
                    HasOpenNode = true;

                if (!HasOpenNode)
                    closed[curr.x, curr.y] = true;
                else
                    return;
            }
        }

        void IPathFInder.UpdateMap()
        {
            for (int x = 0; x < mapXSize; ++x)
            {
                for (int y = 0; y < mapYSize; ++y)
                {
                    jpsNodes[x, y].IsBlocked = grid[x, y].IsBlocked;
                }
            }
        }
    }


}